#include "mpu.h"
#include <basic.h>
#include <math.h>

extern I2C_HandleTypeDef hi2c1;  // 根据你的 CubeMX 配置
#define MPU6050_ADDR (0x68 << 1) // HAL 库使用 8 位地址（左移 1 位）
static HAL_StatusTypeDef MPU6050_WriteReg(uint8_t reg, uint8_t data);
static HAL_StatusTypeDef MPU6050_ReadReg(uint8_t reg, uint8_t *data, uint16_t len);

static mpu_accel_fs_t accel_fs = MPU_2G;
static mpu_gyro_fs_t gyro_fs = MPU_250DPS;
static mpu_euler_angle_t euler_angle = {0};
static mpu_acce_gyro_data_t acce_gyro_data = {0};

#define FILTER_COEFFICIENT 0.95f

void mpu_start(mpu_accel_fs_t _accel_fs, mpu_gyro_fs_t _gyro_fs)
{
    accel_fs = _accel_fs;
    gyro_fs = _gyro_fs;

    // 唤醒 MPU6050（默认上电处于睡眠模式）
    MPU6050_WriteReg(0x6B, 0x00); // PWR_MGMT_1 = 0x00 → 退出睡眠，使用内部时钟

    // 可选：配置加速度计量程（±2g, ±4g, ±8g, ±16g）
    // 0x00: ±2g, 0x08: ±4g, 0x10: ±8g, 0x18: ±16g
    MPU6050_WriteReg(0x1C, accel_fs); // ACCEL_CONFIG

    // 可选：配置陀螺仪量程（±250, ±500, ±1000, ±2000 dps）
    // 0x00: ±250, 0x08: ±500, 0x10: ±1000, 0x18: ±2000
    MPU6050_WriteReg(0x1B, gyro_fs); // GYRO_CONFIG
}

typedef struct
{
    int16_t ax, ay, az;
    int16_t temp;
    int16_t gx, gy, gz;
} MPU6050_Data_t;

static bool MPU6050_ReadAll(MPU6050_Data_t *data)
{
    uint8_t raw[14];
    HAL_StatusTypeDef ret = MPU6050_ReadReg(0x3B, raw, 14);
    if (ret != HAL_OK)
        return false;

    data->ax = (int16_t)(raw[0] << 8 | raw[1]);
    data->ay = (int16_t)(raw[2] << 8 | raw[3]);
    data->az = (int16_t)(raw[4] << 8 | raw[5]);

    data->temp = (int16_t)(raw[6] << 8 | raw[7]);

    data->gx = (int16_t)(raw[8] << 8 | raw[9]);
    data->gy = (int16_t)(raw[10] << 8 | raw[11]);
    data->gz = (int16_t)(raw[12] << 8 | raw[13]);

    return true;
}
int mpu_get_acce_gyro_data(mpu_acce_gyro_data_t *data)
{
    MPU6050_Data_t raw_data;
    if (!MPU6050_ReadAll(&raw_data))
    {
        LOGI("MPU", "MPU6050 Read Failed");
        return -1;
    }

    float acce_lsb = 0, gyro_lsb = 0;
    if (accel_fs == MPU_2G)
        acce_lsb = 32768.0f / 2.0f;
    else if (accel_fs == MPU_4G)
        acce_lsb = 32768.0f / 4.0f;
    else if (accel_fs == MPU_8G)
        acce_lsb = 32768.0f / 8.0f;
    else if (accel_fs == MPU_16G)
        acce_lsb = 32768.0f / 16.0f;

    if (gyro_fs == MPU_250DPS)
        gyro_lsb = 32768.0f / 250.0f;
    else if (gyro_fs == MPU_500DPS)
        gyro_lsb = 32768.0f / 500.0f;
    else if (gyro_fs == MPU_1000DPS)
        gyro_lsb = 32768.0f / 1000.0f;
    else if (gyro_fs == MPU_2000DPS)
        gyro_lsb = 32768.0f / 2000.0f;

#define _FILTER_ALPHA 0.7f

    acce_gyro_data.ax = _FILTER_ALPHA * acce_gyro_data.ax + (1 - _FILTER_ALPHA) * raw_data.ax / acce_lsb;
    acce_gyro_data.ay = _FILTER_ALPHA * acce_gyro_data.ay + (1 - _FILTER_ALPHA) * raw_data.ay / acce_lsb;
    acce_gyro_data.az = _FILTER_ALPHA * acce_gyro_data.az + (1 - _FILTER_ALPHA) * raw_data.az / acce_lsb;

    acce_gyro_data.gx = _FILTER_ALPHA * acce_gyro_data.gx + (1 - _FILTER_ALPHA) * (raw_data.gx / gyro_lsb - 2.674959);
    acce_gyro_data.gy = _FILTER_ALPHA * acce_gyro_data.gy + (1 - _FILTER_ALPHA) * (raw_data.gy / gyro_lsb - 1.632414);
    acce_gyro_data.gz = _FILTER_ALPHA * acce_gyro_data.gz + (1 - _FILTER_ALPHA) * (raw_data.gz / gyro_lsb + 0.085087);

    data->ax = acce_gyro_data.ax;
    data->ay = acce_gyro_data.ay;
    data->az = acce_gyro_data.az;

    data->gx = acce_gyro_data.gx;
    data->gy = acce_gyro_data.gy;
    data->gz = acce_gyro_data.gz;

    return 0;
}
int mpu_get_euler_angle(mpu_euler_angle_t *angle)
{
    angle->pitch = euler_angle.pitch;
    angle->roll = euler_angle.roll;
    angle->yaw = euler_angle.yaw;
    return 0;
}

int mpu_loop(mpu_acce_gyro_data_t *acce_gyro_data, mpu_euler_angle_t *angle, float dt)
{
    int ret = mpu_get_acce_gyro_data(acce_gyro_data);
    if (ret < 0)
        return ret;

    euler_angle.pitch = acce_gyro_data->gy * dt + euler_angle.pitch;
    euler_angle.roll = acce_gyro_data->gx * dt + euler_angle.roll;
    euler_angle.yaw = acce_gyro_data->gz * dt + euler_angle.yaw;

    /*caculate the angle use acce_data*/
    float roll = atan2(acce_gyro_data->ay, acce_gyro_data->az) * 180.0f / M_PI;
    float pitch = atan2(-acce_gyro_data->ax, sqrt(acce_gyro_data->ay * acce_gyro_data->ay + acce_gyro_data->az * acce_gyro_data->az)) * 180.0f / M_PI;

    /*Ensure the real time performance of the filter*/
    euler_angle.pitch = FILTER_COEFFICIENT * euler_angle.pitch + (1 - FILTER_COEFFICIENT) * pitch;
    euler_angle.roll = FILTER_COEFFICIENT * euler_angle.roll + (1 - FILTER_COEFFICIENT) * roll;

    angle->pitch = euler_angle.pitch;
    angle->roll = euler_angle.roll;
    angle->yaw = euler_angle.yaw;

    return ret;
}

void mpu_test_software()
{
    serial_send("MPU6050 Test Software\r\n");
    mpu_start(MPU_2G, MPU_250DPS);

    mpu_acce_gyro_data_t acce_gyro_data;
    mpu_euler_angle_t euler_angle;

    for (;;)
    {
        __PERIOD(mpu_test, 5,
                 {
                     uint32_t us = get_us();
                     mpu_loop(&acce_gyro_data, &euler_angle, 0.05f);
                     uint32_t delta_us = get_us() - us;

                     serial_send("delta_us= ");
                     print_int(delta_us);

                     //  print_float(euler_angle.pitch, 4);
                     //  serial_send_char(',');
                     //  print_float(euler_angle.roll, 4);
                     //  serial_send_char(',');
                     //  print_float(euler_angle.yaw, 4);
                     serial_send_char('\n');
                 });
    }
}

HAL_StatusTypeDef MPU6050_WriteReg(uint8_t reg, uint8_t data)
{
    uint8_t buf[2] = {reg, data};
    return HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, buf, 2, 100);
}

HAL_StatusTypeDef MPU6050_ReadReg(uint8_t reg, uint8_t *data, uint16_t len)
{
    HAL_StatusTypeDef ret = HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, &reg, 1, 100);
    if (ret != HAL_OK)
        return ret;
    return HAL_I2C_Master_Receive(&hi2c1, MPU6050_ADDR, data, len, 100);
}
